#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <algorithm>
#include "rtubuffer.h"
#include "../jsonconfig/jsonconfig.h"
using namespace std;

RTUBbuffer buffer;
std::vector<PollingCode> pollingqueue;

static void log(string msg, int level)
{
    if (!config.debuglog)
        return;
    if (level == 0 && level >= config.loglevel)
        cout << "\033[36m[rtubuffer]\033[0m" << msg << endl;
    else if (level == 1 && level >= config.loglevel)
        cout << "\033[36m[rtubuffer]\033[0m"
             << "\033[36m" << msg << "\033[0m" << endl;
    else if (level >= 2)
        cout << "\033[36m[rtubuffer]\033[0m"
             << "\033[31m" << msg << "\033[0m" << endl;
}

/**
 * @brief 初始化寄存器缓冲区，并生成轮询指令队列
 * 
 */
void init_buffer()
{
    PollingCode code;
    uint16_t last_addr;
    bool firstaddr = true;
    buffer.clear();
    for (PointList::iterator dev = config.points.begin(); dev != config.points.end(); dev++)
    {
        uint8_t slave = dev->first;
        auto funclist = dev->second;
        map<uint8_t, map<uint16_t, RegDataType>> funcregmem;
        for (auto reglist = funclist.begin(); reglist != funclist.end(); reglist++)
        {
            map<uint16_t, RegDataType> regmem;
            uint8_t func = reglist->first;
            auto addrlist = reglist->second;
            for (auto addr = addrlist.begin(); addr != addrlist.end(); addr++)
            {
                RegDataType reg;
                reg.data = 0x0000;
                reg.status = 0xF0;
                regmem.insert(pair<uint16_t, RegDataType>(*addr, reg));
                //下方代码用于将最多连续的maxnb个寄存器合并为单次读取
                if (firstaddr)
                { //每个<slave,func>的第一组addr
                    last_addr = *addr;
                    code.addr = *addr;
                    code.func = func;
                    code.nb = 1;
                    code.slave = slave;
                    firstaddr = false;
                }
                else if (*addr == last_addr + 1 && code.nb < config.bridgeconfig.maxnb)
                { //地址连续且未达到最大读取数目则合并为一个命令
                    last_addr++;
                    code.nb++;
                }
                else
                { //提交前一个命令并新建命令
                    pollingqueue.push_back(code);
                    last_addr = *addr;
                    code.addr = *addr;
                    code.func = func;
                    code.nb = 1;
                    code.slave = slave;
                }
            }
            pollingqueue.push_back(code);
            firstaddr = true;
            funcregmem.insert(pair<uint8_t, map<uint16_t, RegDataType>>(func, regmem));
        }
        buffer.insert(pair<uint8_t, map<uint8_t, map<uint16_t, RegDataType>>>(slave, funcregmem));
    }
    log("buffer init", 0);
}

/**
 * @brief 向缓冲区buffer中写入单个字节
 * 
 * @param slave 从机SlaveID
 * @param func 功能区（3 only）
 * @param addr 寄存器地址
 * @param data 写入值
 */
bool ModbusBufferWriteByte(uint8_t slave, uint8_t func, uint16_t addr, uint16_t data)
{
    bool exist = false;
    try
    {
        buffer.at(slave).at(func).at(addr).data = data;
        if(buffer.at(slave).at(func).at(addr).status >= 0xFE)
            buffer.at(slave).at(func).at(addr).status = 0xFE;
        else
            buffer.at(slave).at(func).at(addr).status = 0;
        exist = true;
    }
    catch (...)
    {
        ModbusBufferInsert(slave, func, addr, data);
        string msg;
        log("Reg not Exitst", 1);
    }
    return exist;
}

void ModbusBufferSetByteMiss(uint8_t slave, uint8_t func, uint16_t addr)
{
    try
    {
        //需要保证不会加到0xFE
        if (buffer.at(slave).at(func).at(addr).status < 0xF0)
            buffer.at(slave).at(func).at(addr).status++;
    }
    catch (...)
    {
        log("Reg not Exitst", 1);
    }
}

/**
 * @brief 设置寄存器数据读取失败（未更新）
 * 
 * @param slave SlaveID
 * @param func Function
 * @param addr 寄存器开始地址
 * @param nb 连续字节数
 */
void ModbusBufferSetMiss(uint8_t slave, uint8_t func, uint16_t addr, int nb)
{
    for (int i = 0; i < nb; i++)
        ModbusBufferSetByteMiss(slave, func, addr + i);
}

bool ModbusBufferReadByte(uint8_t slave, uint8_t func, uint16_t addr, uint16_t *data)
{
    bool exist = false;
    try
    {
        *data = buffer.at(slave).at(func).at(addr).data;
        if (buffer.at(slave).at(func).at(addr).status < 20)
            exist = true;
        else if(buffer.at(slave).at(func).at(addr).status == 0xFE)
        {
            exist = true;
            buffer.at(slave).at(func).at(addr).status = 0xFF;
        }
        else
        {
            log("Reg value out of date", 1);
            printf("ADDR:%02d-%02d-%04d",slave,func,addr);
        }
    }
    catch (...)
    {
        *data = 0;
        log("Reg not Exitst", 1);
    }
    return exist;
}

bool ModbusBufferWrite(uint8_t slave, uint8_t func, uint16_t addr, int nb, uint16_t *data)
{
    bool allexist = true;
    for (int i = 0; i < nb; i++)
        allexist = ModbusBufferWriteByte(slave, func, addr + i, *(data + i)) && allexist;
    return allexist;
}

bool ModbusBufferRead(uint8_t slave, uint8_t func, uint16_t addr, int nb, uint16_t *data)
{
    bool allexist = true;
    for (int i = 0; i < nb; i++)
        allexist = ModbusBufferReadByte(slave, func, addr + i, (data + i)) && allexist;
    return allexist;
}

void ModbusBufferInsert(uint8_t slave, uint8_t func, uint16_t addr, uint16_t data)
{
    auto it_slave = buffer.find(slave);
    if(it_slave == buffer.end())
    {
        map<uint8_t, map<uint16_t, RegDataType>> map_func;
        buffer.insert(pair<uint8_t, map<uint8_t, map<uint16_t, RegDataType>>>(slave,map_func));
        it_slave = buffer.find(slave);
    }
    auto it_func = it_slave->second.find(func);
    if(it_func == it_slave->second.end())
    {
        map<uint16_t, RegDataType> map_addr;
        it_slave->second.insert(pair<uint8_t, map<uint16_t, RegDataType>>(func,map_addr));
        it_func = it_slave->second.find(func);
    }
    auto it_addr = it_func->second.find(addr);
    if(it_addr == it_func->second.end())
    {
        RegDataType datanew;
        datanew.data = data;
        datanew.status = 0xFE;
        it_func->second.insert(pair<uint16_t,RegDataType>(addr,datanew));
    }
    else
    {
        it_addr->second.data = data;
        it_addr->second.status = 0xFE;
    }
}